#include "drone.h"

namespace drone_coord
{
    Matrix3x3 create_rotation_matrix(double yaw_deg, double pitch_deg, double roll_deg)
    {
        // 计算各个轴的旋转矩阵
        double cy = cos(yaw_deg);
        double sy = sin(yaw_deg);
        double cp = cos(pitch_deg);
        double sp = sin(pitch_deg);
        double cr = cos(roll_deg);
        double sr = sin(roll_deg);

        // 构建ZYX顺序的旋转矩阵
        double data[3][3] = {
            {cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr},
            {sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr},
            {-sp, cp * sr, cp * cr}
        };

        return Matrix3x3(data);
    }

    bool pixel_to_global(
        const Vector3d &pixel,
        const Vector3d &global_pos,
        const Matrix3x3 &drone_rot,
        const Matrix3x3 &camera_rot,
        const CameraIntrinsics &intrinsics,
        Vector3d &result,
        Vector3d &ray_drone_out,  // 新增输出参数：射线方向  调试使用
        double &t_out) {         // 新增输出参数：t值       调试使用
        // 1. 像素坐标转换为相机坐标系下的射线方向
        double z = -1.0; // 射线朝下
        // 图像 y 表示相机上下方向，影响 X 轴（前方）
        // 图像 x 表示相机左右方向，影响 Y 轴（左/右）
        double y_camera = -(pixel.x - intrinsics.cx) / intrinsics.fx; // 图像 x → 无人机 y
        double x_camera = -(pixel.y - intrinsics.cy) / intrinsics.fy; // 图像 y → 无人机 x

        // 迭代法反向校正, 矫正相机畸变
        const int MAX_ITER = 5;
        double x_corrected = x_camera;
        double y_corrected = y_camera;
        
        for(int i = 0; i < MAX_ITER; ++i) {
            double r2 = x_corrected * x_corrected + y_corrected * y_corrected;
            double factor = 1 + intrinsics.k1 * r2 + intrinsics.k2 * r2 *r2;
            
            x_corrected = x_camera / factor;
            y_corrected = y_camera / factor;
        }

        x_camera = x_corrected;
        y_camera = y_corrected;

        // 2. 单位方向向量
        double norm = sqrt(x_camera*x_camera + y_camera*y_camera + z*z);
        x_camera /= norm;
        y_camera /= norm;
        z /= norm;

        // 3. 考虑相机姿态（外参）
        Vector3d ray_camera(x_camera, y_camera, z);
        Vector3d ray_drone = drone_rot.multiply(camera_rot.multiply(ray_camera));

        // 4. 计算射线与地面的交点
        if (ray_drone.z >= 0 || std::fabs(ray_drone.z) < 1e-8) {
            return false; // 射线方向向上或垂直，无法与地面相交
        }

        double t = -global_pos.z / ray_drone.z;
        result.x = global_pos.x + t * ray_drone.x;
        result.y = global_pos.y + t * ray_drone.y;
        result.z = 0.0;

        // 输出射线方向和 t 值
        ray_drone_out = ray_drone;
        t_out = t;

        return true;
    }

    Vector3d get_global_pos(Vector3d &drone_pos, Vector3d &camera_pos, Matrix3x3 &drone_rot)
    {
        // 相机在无人机坐标系中的位置，通过旋转后的位置加上无人机全局位置
        Vector3d rotated_camera_pos = drone_rot.multiply(camera_pos);
        return Vector3d(
            drone_pos.x + rotated_camera_pos.x,
            drone_pos.y + rotated_camera_pos.y,
            drone_pos.z + rotated_camera_pos.z);
    }

} // namespace drone_coord